#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "cyhal.h"
#include "adc.h"

int32_t sim_data_buffer[1000];
int32_t p_data[1000];
int arr_rowsum[500];

int max_window_length;

double gaussrand_normal()
{
    static double v1,v2,s;
    static int phase = 0;
    double x;
    if(phase == 0)
    {
        do
        {
            double u1 = (double)rand()/ RAND_MAX;
            double u2 = (double)rand()/ RAND_MAX;
            v1 = 2*u1-1;
            v2 = 2*u2-1;
            s=v1*v1 + v2*v2;
        }while((s>=1) || (s==0));
        x = v1*sqrt(-2*log(s)/s);
    }
    else
    {
        x = v2*sqrt(-2*log(s)/s);
    }

    phase = 1-phase;
    return x;
}

double gaussrand(double mean, double stdc)
{
    return mean+gaussrand_normal()*stdc;
}

void linespace(int32_t x1,int32_t x2, int n, int32_t* out)
{
    int i=0;
    double d=(x2-x1)/(n-1);
    for(i=0; i<n; i++)
    {
        out[i] = x1 + i*d;
    }
}


void sim_data()
{
    linespace(0,200, 1000, sim_data_buffer);
    for(int i=0; i<1000; i++)
    {
        sim_data_buffer[i] = 2 * cos(2 * M_PI * 300 * sim_data_buffer[i]) \
            + 5 * sin(2 * M_PI * 100 * sim_data_buffer[i]) \
            + 4 * gaussrand(0,1);
    }
}

int argmin(int* buffer, int len)
{
    int index = 0;
    int32_t tmp = buffer[0];
    for(int i=0; i<len; i++)
    {
        if(tmp > buffer[i])
        {
            tmp = buffer[i];
            index =i;
        }
    }
    return index;
}

void ampd(int32_t* data, int32_t len)
{
    int row_sum;
    for(int k=1; k<len/2+1; k++)
    {
        row_sum = 0;
        for(int i=k; i<len-k; i++)
        {
            if((data[i] > data[i - k]) && (data[i] > data[i + k]))
            {
                row_sum -= 1;
            }
        }
        arr_rowsum[k-1] = row_sum;
    }
    int min_index = argmin(arr_rowsum,len/2+1);
     max_window_length = min_index;
    for(int k=1; k<max_window_length + 1; k++)
    {
        for(int i=k; i<len - k; i++)
        {
            if((data[i] > data[i - k]) && (data[i] > data[i + k]))
            {
                p_data[i] += 1;
            }
        }
    }

    for(int k=0; k<len; k++)
    {
        if(p_data[k] == max_window_length)
        {
            /* 极大值 */
        }
    }
}

int max_test(void)
{
	for(int i=0; i<10; i++)
	{
		memset(p_data,0,sizeof(p_data));
		//adc_samp(sim_data_buffer,1000);
		sim_data();
		ampd(sim_data_buffer, sizeof(sim_data_buffer)/sizeof(sim_data_buffer[0]));
	    for(int k=0; k<sizeof(sim_data_buffer)/sizeof(sim_data_buffer[0]); k++)
	    {
	        if(p_data[k] == max_window_length)
	        {
	            /* 极大值 */
	        	printf("/*%ld,%ld*/\r\n",sim_data_buffer[k],sim_data_buffer[k]);
	        }
	        else
	        {
	        	printf("/*%ld,%d*/\r\n",sim_data_buffer[k],0);
	        }
	        cyhal_system_delay_ms(10);
	    }
	}
	return 0;
}
